package pack.help;

import gui.Main;
import pack.model.RealLifeProperties;

public class RobotCommand {

	private Direction dirL, dirR;
	
	private int speedL, speedR;
	private int stepsL, stepsR;
	
	private int prevSpeedL, prevSpeedR;
	
	private final int maxSp = 255;
	
	public enum Direction{
		FORWARD, BACKWARD;
	}
	
	public RobotCommand(Direction dirL, Direction dirR, int speedL, int speedR, int stepsL, int stepsR){
		this.dirL = dirL;
		this.dirR = dirR;
		this.speedL = speedL;
		this.speedR = speedR;
		this.stepsL = stepsL;
		this.stepsR = stepsR;
		this.prevSpeedL = 255;
		this.prevSpeedR = 255;
	}
	
	public RobotCommand(RobotCommand cmd){
		this.dirL = cmd.dirL;
		this.dirR = cmd.dirR;
		this.speedL = cmd.speedL;
		this.speedR = cmd.speedR;
		this.stepsL = cmd.stepsL;
		this.stepsR = cmd.stepsR;
		this.prevSpeedL = cmd.prevSpeedL;
		this.prevSpeedR = cmd.prevSpeedR;
	}
	
	public RobotCommand(Command cmd){
		float v1 = cmd.getV1();
		float v2 = cmd.getV2();
		
		double vL = v1;
		double vR = v2;
		
		this.prevSpeedL = Main.prevSpL;
		this.prevSpeedR = Main.prevSpR;
		
		this.dirL = Direction.FORWARD;
		this.dirR = Direction.FORWARD;
		if (v1<0) { this.dirL = Direction.BACKWARD; v1 = -v1; }
		if (v2<0) { this.dirR = Direction.BACKWARD; v2 = -v2; }
		
		float timeS = cmd.getTime()/1000.f;

		this.speedL = RealLifeProperties.vToSpeed(vL);
		this.speedR = RealLifeProperties.vToSpeed(vR);
		
		double tAccL = getAccTime(speedL,prevSpeedL);
		double tAccR = getAccTime(speedR,prevSpeedR);
		
		if (tAccL <= timeS ){
			this.stepsL = (int) Math.round((timeS-tAccL)*RealLifeProperties.timerFrequency/(speedL*2)) + Math.abs(prevSpeedL-speedL);
		} else {
			int dir = 1;
			if (speedL > prevSpeedL){
				dir = -1;
			}
			
			while (tAccL > timeS){
				speedL += dir;
				tAccL = getAccTime(speedL,prevSpeedL);
			}
			
			this.stepsL = (int) Math.round((timeS-tAccL)*RealLifeProperties.timerFrequency/(speedL*2)) + Math.abs(prevSpeedL-speedL);
			//speedL -= dir;
		}
		
		if (tAccR <= timeS){
			this.stepsR = (int) Math.round((timeS-tAccR)*RealLifeProperties.timerFrequency/(speedR*2)) + Math.abs(prevSpeedR-speedR);
		} else {
			int dir = 1;
			if (speedR > prevSpeedR){
				dir = -1;
			}
			
			while (tAccR > timeS){
				speedR += dir;
				tAccR = getAccTime(speedR,prevSpeedR);
			}
			
			this.stepsR = (int) Math.round((timeS-tAccR)*RealLifeProperties.timerFrequency/(speedR*2)) + Math.abs(prevSpeedR-speedR);
			//speedR -= dir;
		}
		
		if (stepsL==0) speedL = 0;
		if (stepsR==0) speedR = 0;
		
		if (vL == 0){
			stepsL = maxSp-prevSpeedL;
			speedL = maxSp;
		}
		if (vR == 0){
			stepsR = maxSp-prevSpeedR;
			speedR = maxSp;
		}
		

		
	}
	
	public RobotCommand(String cmd){
		if (cmd.charAt(4) == 'f'){
			dirL = Direction.FORWARD;
		} else {
			dirL = Direction.BACKWARD;
		}
		
		if (cmd.charAt(5) == 'f'){
			dirR = Direction.FORWARD;
		} else {
			dirR = Direction.BACKWARD;
		}
		
		speedL = Integer.parseInt(cmd.substring( 7, 10));
		speedR = Integer.parseInt(cmd.substring(11, 13));
		
		stepsL = Integer.parseInt(cmd.substring(14, 18));
		stepsR = Integer.parseInt(cmd.substring(19, 23));
		
	}
	
	public double getAccTime(int sp, int prevSp){
		double accL = (sp*(sp+1)/2.0)-(prevSp*(prevSp+1)/2.0);
		accL *= 2;
		
		if (sp < prevSp){
			accL *= -1;
		}
	
		accL /= RealLifeProperties.timerFrequency;
		return accL;
	}
	
	public Direction getDirL(){
		return dirL;
	}
	
	public Direction getDirR() {
		return dirR;
	}

	public int getSpeedL() {
		return speedL;
	}

	public int getSpeedR() {
		return speedR;
	}

	public int getStepsL() {
		return stepsL;
	}

	public int getStepsR() {
		return stepsR;
	}

}
